#include "./sensor.h"

#include <Wire.h>
#include "driver/i2c.h"
#include "../core/uart/uart.h"

#define MIN(X, Y) ((X > Y) ? (Y) : (X))

// 取前四位 灵敏度
#define CH_SENS(a, b) (((a & 0x0F) << 4) + (b & 0x0F))

#define LF 0x0A // New Line
#define CR 0x0D // Carriage  return
#define SP 0x20 // Spcae

typedef enum
{
    SENS1 = 0x02, // Sensitivity, Channel 2 and 1
    SENS2 = 0x03, // Channel 4 and 3
    SENS3 = 0x04, // Channel 6 and 5
    SENS4 = 0x05, // Channel 8 and 7
    SENS5 = 0x06, // Channel 10 and 9
    SENS6 = 0x07, // Channel 12 and 11
    CTRL1 = 0x08,
    CTRL2 = 0x09,
    REF_RST1 = 0x0A,
    REF_RST2 = 0x0B,
    CH_HOLD1 = 0x0C,
    CH_HOLD2 = 0x0D,
    CAL_HOLD1 = 0x0E,
    CAL_HOLD2 = 0x0F,
    OUTPUT1 = 0x10,
    OUTPUT2 = 0x11,
    OUTPUT3 = 0x12,
} TSM_RegMapAddr_TypeDef;

#define OUTPUT_REG1 0x10 // cs1~cs4 output
#define OUTPUT_REG2 0x11 // cs5~cs8 output
#define OUTPUT_REG3 0x12 // cs9~cs12 output

// FROM: https://github.com/yni2yni/TSM12/blob/master/ADS_Touch_Sensor_TSM12.ino

bool Sensor::touch_wire_init = false;

TwoWire i2c_touch = TwoWire(1);

xy_axis Sensor::touch_bar[STORAGE];

touch_s Sensor::touch_SPL[2] = {TOUCH_SPL_DEF}; //  0->num1 1->num2  LSB4->灵敏度 MAX 15 MIN 0  MSB4->压感触发阈值 MAX 2 MIN 0

int32_t Sensor::mai_map[TOUCH_NUM][2]; // serial点位映射

int8_t Sensor::sample_time = 0;

uint32_t Sensor::touch_refence_map = SAVE_INT32_1;
uint32_t Sensor::_old_touch_refence_map = SAVE_INT32_1;
uint32_t Sensor::touch_refence_lock = 0;

// 用于将灵敏度重新映射为0(max) 15(min)的状态 应当只处理输入和显示数据
// real:8(max) -> 15(min+) && 0(max-) -> 7(min)
// refect -> 0(max) -> 15(min)
static const uint8_t transform_dict[16] = {
    8, 0, 9, 1, 10, 2, 11, 3, 12, 4, 13, 5, 14, 6, 15, 7};
static const uint8_t _transform_dict[16] = {
    1, 3, 5, 7, 9, 11, 13, 15, 0, 2, 4, 6, 8, 10, 12, 14};

inline uint8_t sensitivity_transform(uint8_t input, uint8_t recover = 0)
{
    if (recover)
        return _transform_dict[input];
    else
        return transform_dict[input];
}

static byte out_reg[3] = { // REG
    OUTPUT_REG1,
    OUTPUT_REG2,
    OUTPUT_REG3};

inline void TSM_Write_Byte(uint8_t addr, uint8_t data, uint8_t p_addr)
{
    i2c_touch.beginTransmission(p_addr); // sned ic slave address
    i2c_touch.write(addr);
    i2c_touch.write(data);
    i2c_touch.endTransmission(); // stop transmitting
}

inline void TSM_Write_2Byte(uint8_t addr, uint8_t lsb, uint8_t msb, uint8_t p_addr)
{
    i2c_touch.beginTransmission(p_addr); // sned ic slave address
    i2c_touch.write(addr);
    i2c_touch.write(lsb);
    i2c_touch.write(msb);
    i2c_touch.endTransmission(); // stop transmitting
}

void Sensor::set_sample_time(uint8_t time) {
    if (time > 0b111) {
        uart.send("ERROR:SAMPLE TIME MUST IN 0 - 7");
        uart.send("ERROR:CURRENT SAMPLE TIME:", 0);
        uart.send(sensor.sample_time);
        return;
    }
    uart.send("INFO:SAMPLE TIME SET:", 0);
    uart.send(time);
    sensor.sample_time = time;
    sensor.tsm12mc_reset();
    sensor.start_sample();
}

uint8_t Sensor::i2c_scan()
{
    uart.send("SCAN_START", 1);
    uint8_t num = 0;
    for (uint8_t addr = 1; addr < 127; addr++)
    {
        i2c_touch.beginTransmission(addr);
        uint8_t error = i2c_touch.endTransmission();
        if (error == 0)
        {
            uart.send("GET_ADDRESS:", 0);
            uart.send(addr, 1);
            if (addr != TSM12MC_ADD1 && addr != TSM12MC_ADD2)
            {
                uart.send("ERROR:ADDRESS_FAILED");
                return 128;
            }
            num++;
        }
        else if (error == 4)
        {
            uart.send("GET_UNKNOW:", 0);
            uart.send(addr, 1);
            num += 0x10;
        }
    }
    uart.send("SCAN_END", 1);
    return num;
}

// 设置自校正 false关闭
void Sensor::tsm12_refence(uint8_t addr, uint16_t map)
{
    TSM_Write_Byte(CAL_HOLD1, map | 0xFF, addr);
    TSM_Write_Byte(CAL_HOLD2, map | 0xF00, addr);
}

void Sensor::touch_refence_update(uint32_t lock, bool reset)
{
    touch_refence_map |= (lock & (0xFFFFFFFF >> (32 - TOUCH_NUM)));
    if ((touch_refence_lock == 0xFFFFFFFF) || reset)
        touch_refence_map = SAVE_INT32_1; // bit 32 = 1
    touch_refence_map &= ~touch_refence_lock;
    if (_old_touch_refence_map == touch_refence_map)
        return;
    if (!touch_wire_init || !touch_refence_map || lock > TOUCH_NUM)
        return;
    tsm12_refence(TSM12MC_ADD1, touch_refence_map & 0xFFF);
    tsm12_refence(TSM12MC_ADD2, (touch_refence_map >> 12) & 0xFFF);
    _old_touch_refence_map = touch_refence_map;
}

// ID: 0->0xD0 1->0xF0
uint8_t Sensor::tsm12mc_read(uint8_t id, uint32_t *read)
{
    if (!touch_wire_init)
        return 0;
    i2c_touch.flush();
    uint16_t _address = ((id == 1) ? TSM12MC_ADD2 : TSM12MC_ADD1);
    i2c_touch.beginTransmission(_address);
    i2c_touch.write(out_reg[0]);
    if (!i2c_touch.endTransmission())
    {
        i2c_touch.requestFrom(_address, (uint8_t)3);
        if (!i2c_touch.available())
            return 1;
        *read = 0;
        for (uint8_t cc = 0; cc < 3; cc++)
        {
            (*read) |= (uint32_t)((i2c_touch.read() & 0xFF) << (8 * cc));
        }
        return 0;
    }
    return 1;
}

inline void tsm12init(uint8_t addr)
{
    uint8_t ic_id = 0;
    if (addr == TSM12MC_ADD2)
        ic_id = 1;
    TSM_Write_Byte(CTRL2, 0x0B, addr);
    TSM_Write_Byte(SENS1, CH_SENS(sensor.touch_SPL[ic_id].sst[2], sensor.touch_SPL[ic_id].sst[1]), addr);   // 2, 1
    TSM_Write_Byte(SENS2, CH_SENS(sensor.touch_SPL[ic_id].sst[4], sensor.touch_SPL[ic_id].sst[3]), addr);   // 4, 3
    TSM_Write_Byte(SENS3, CH_SENS(sensor.touch_SPL[ic_id].sst[6], sensor.touch_SPL[ic_id].sst[5]), addr);   // 6, 5
    TSM_Write_Byte(SENS4, CH_SENS(sensor.touch_SPL[ic_id].sst[8], sensor.touch_SPL[ic_id].sst[7]), addr);   // 8, 7
    TSM_Write_Byte(SENS5, CH_SENS(sensor.touch_SPL[ic_id].sst[10], sensor.touch_SPL[ic_id].sst[9]), addr);  // A, 9
    TSM_Write_Byte(SENS6, CH_SENS(sensor.touch_SPL[ic_id].sst[12], sensor.touch_SPL[ic_id].sst[11]), addr); // C ,B
    TSM_Write_Byte(CTRL1, 0b00101000 | (sensor.sample_time & 0b111), addr);                                 // AUTO MODE +
    TSM_Write_2Byte(REF_RST1, 0xFF, 0x0F, addr);
    TSM_Write_2Byte(CH_HOLD1, 0x00, 0x00, addr); // enable all channels
    TSM_Write_Byte(CAL_HOLD1, 0x00, addr);       // No Calibration
    TSM_Write_Byte(CAL_HOLD2, 0x00, addr);
    TSM_Write_Byte(CTRL2, 0x03, addr);

    // TSM_Write_Byte(CTRL2, 0x0F, addr);                                                                      // enable sleep mode
    // TSM_Write_Byte(SENS1, CH_SENS(sensor.touch_SPL[ic_id].sst[2], sensor.touch_SPL[ic_id].sst[1]), addr);   // 9, 6
    // TSM_Write_Byte(SENS2, CH_SENS(sensor.touch_SPL[ic_id].sst[4], sensor.touch_SPL[ic_id].sst[3]), addr);   // 3, #
    // TSM_Write_Byte(SENS3, CH_SENS(sensor.touch_SPL[ic_id].sst[6], sensor.touch_SPL[ic_id].sst[5]), addr);   // 8, 5
    // TSM_Write_Byte(SENS4, CH_SENS(sensor.touch_SPL[ic_id].sst[8], sensor.touch_SPL[ic_id].sst[7]), addr);   // 2, 0
    // TSM_Write_Byte(SENS5, CH_SENS(sensor.touch_SPL[ic_id].sst[10], sensor.touch_SPL[ic_id].sst[9]), addr);  // *, 1
    // TSM_Write_Byte(SENS6, CH_SENS(sensor.touch_SPL[ic_id].sst[12], sensor.touch_SPL[ic_id].sst[11]), addr); // 4, 7
    // TSM_Write_Byte(CTRL1, 0b10001000, addr);                                                                // FAST MODE +
    // TSM_Write_2Byte(REF_RST1, 0x00, 0x00, addr);                                                            // renew value of everyone channel
    // TSM_Write_2Byte(CH_HOLD1, 0x00, 0x00, addr);                                                            // enable all channels
    // TSM_Write_2Byte(CH_HOLD2, 0x00, 0x00, addr);
    // TSM_Write_Byte(CAL_HOLD1, 0x00, addr); // No Calibration
    // TSM_Write_Byte(CAL_HOLD2, 0x00, addr);
    // TSM_Write_Byte(CTRL2, 0b00000011, addr);
}

void Sensor::tsm12mc_settings_update(uint8_t addr)
{
    uint8_t ic_id = 0;
    if (addr == TSM12MC_ADD2)
        ic_id = 1;
    TSM_Write_Byte(SENS1, CH_SENS(sensor.touch_SPL[ic_id].sst[2], sensor.touch_SPL[ic_id].sst[1]), addr);   // 9, 6
    TSM_Write_Byte(SENS2, CH_SENS(sensor.touch_SPL[ic_id].sst[4], sensor.touch_SPL[ic_id].sst[3]), addr);   // 3, #
    TSM_Write_Byte(SENS3, CH_SENS(sensor.touch_SPL[ic_id].sst[6], sensor.touch_SPL[ic_id].sst[5]), addr);   // 8, 5
    TSM_Write_Byte(SENS4, CH_SENS(sensor.touch_SPL[ic_id].sst[8], sensor.touch_SPL[ic_id].sst[7]), addr);   // 2, 0
    TSM_Write_Byte(SENS5, CH_SENS(sensor.touch_SPL[ic_id].sst[10], sensor.touch_SPL[ic_id].sst[9]), addr);  // *, 1
    TSM_Write_Byte(SENS6, CH_SENS(sensor.touch_SPL[ic_id].sst[12], sensor.touch_SPL[ic_id].sst[11]), addr); // 4, 7
}

void Sensor::nvs_touch_spl_set(uint8_t id, int8_t target)
{
    uint8_t ic_id = 0;
    if (id > TOUCH_NUM)
        return;
    if (id > 11)
    {
        id -= 12;
        ic_id = 1;
    }
    sensor.touch_SPL[ic_id].sst[id] = target;
}

int8_t Sensor::nvs_touch_spl_read(uint8_t id)
{
    uint8_t ic_id = 0;
    if (id > TOUCH_NUM)
        return 127;
    if (id > 11)
    {
        id -= 12;
        ic_id = 1;
    }
    return sensor.touch_SPL[ic_id].sst[id];
}

// 设置并更新单点灵敏度 (数据经过转换)
void Sensor::touch_spl_set(uint8_t id, int8_t target)
{
    uint8_t _id = id;
    uint8_t ic_id = 0;
    uint8_t _target = target;
    if (id > TOUCH_NUM)
        return;
    if (id > 11)
    {
        _id -= 12;
        ic_id = 1;
    }
    if (_target < 0)
        _target = 0;
    if (_target > 15)
        _target = 15;

    sensor.touch_SPL[ic_id].sst[_id] &= 0xF0;
    sensor.touch_SPL[ic_id].sst[_id] |= sensitivity_transform(_target);
    tsm12mc_settings_update(ic_id);
}

// 设置目标压感触发强度
void Sensor::touch_pressure_set(uint8_t id, int8_t target)
{
    uint8_t _id = id;
    uint8_t ic_id = 0;
    uint8_t _target = target;
    if (id > TOUCH_NUM)
        return;
    if (id > 11)
    {
        _id -= 12;
        ic_id = 1;
    }
    if (_target < 0)
        _target = 0;
    if (_target > 2)
        _target = 2;

    sensor.touch_SPL[ic_id].sst[_id] &= 0x0F;
    sensor.touch_SPL[ic_id].sst[_id] |= (_target << 4);
}

// 读取目标点灵敏度 (数据经过转换)
int8_t Sensor::touch_spl_read(uint8_t id)
{
    uint8_t _id = id;
    uint8_t ic_id = 0;
    if (id > TOUCH_NUM)
        return 127;
    if (id > 11)
    {
        _id -= 12;
        ic_id = 1;
    }
    return sensitivity_transform(sensor.touch_SPL[ic_id].sst[_id] & 0x0F, 1);
}

// 读取目标压感
int8_t Sensor::touch_pressure_read(uint8_t id)
{
    uint8_t _id = id;
    uint8_t ic_id = 0;
    if (id > TOUCH_NUM)
        return 0;
    if (id > 11)
    {
        _id -= 12;
        ic_id = 1;
    }
    return sensor.touch_SPL[ic_id].sst[_id] >> 4;
}

// 采样控制
void Sensor::start_sample()
{
    TSM_Write_2Byte(REF_RST1, 0x00, 0x00, TSM12MC_ADD1);
    TSM_Write_2Byte(REF_RST1, 0x00, 0x00, TSM12MC_ADD2);
    vTaskDelay(TOUCH_INIT_TIME);
}

void Sensor::tsm12mc_reset()
{
    uint8_t current_state = sensor.touch_wire_init;
    sensor.touch_wire_init = false;
    sensor.touch_refence_map = SAVE_INT32_1;
    sensor._old_touch_refence_map = SAVE_INT32_1;
    vTaskDelay(TOUCH_INIT_TIME / 10);
    planner.set_GPIO(TSM12MC_INT1, 1);
    planner.set_GPIO(TSM12MC_EN1, 1);
    planner.set_GPIO(TSM12MC_INT2, 1);
    planner.set_GPIO(TSM12MC_EN2, 1);
    vTaskDelay(TOUCH_INIT_TIME);
    planner.set_GPIO(TSM12MC_RESET, 0);
    vTaskDelay(TOUCH_REFENCE_TIME);
    // planner.set_GPIO_pull(TSM12MC_RESET, GPIO_PULLDOWN_ONLY);
    planner.set_GPIO(TSM12MC_RESET, 1);
    vTaskDelay(TOUCH_INIT_TIME);
    planner.set_GPIO(TSM12MC_EN1, 0);
    planner.set_GPIO(TSM12MC_EN2, 0);
    tsm12init(TSM12MC_ADD1);
    tsm12init(TSM12MC_ADD2);
    sensor.touch_wire_init = current_state;
    uart.send("INFO:TOUCH RESET OK");
}

void Sensor::sensor_bus_init()
{
    i2c_touch.setPins(TSM12MC_SDA, TSM12MC_SCL);
    i2c_touch.setClock(400000);
    i2c_touch.setBufferSize(96);
    i2c_touch.begin();
    // i2c_scan();
    vTaskDelay(1);
    touch_wire_init = true;
}

// inline esp_err_t esp32_i2c_init()
// {
//     // 初始化I2C配置
//     i2c_config_t i2c_config =
//         {
//             .mode = I2C_MODE_MASTER,             // 设置i2c模式
//             .sda_io_num = TSM12MC_SDA,           // 设置SDA引脚
//             .scl_io_num = TSM12MC_SCL,           // 设置SCL引脚
//             .sda_pullup_en = GPIO_PULLUP_ENABLE, // 设置上拉使能
//             .scl_pullup_en = GPIO_PULLUP_ENABLE, // 设置上拉使能
//         };
//     i2c_config.master.clk_speed = 200000; // 设置时钟频率400kbit
//     // 设置I2C
//     i2c_param_config(1, &i2c_config);
//     // 注册I2C服务及使能
//     esp_err_t err = i2c_driver_install(1, i2c_config.mode, 0, 0, 0);
//     return err;
// }

void Sensor::clean_mai_map()
{
    for (uint8_t ct = 0; ct < TOUCH_NUM; ct++)
    {
        sensor.mai_map[ct][0] = 0;
        sensor.mai_map[ct][1] = 0;
    }
}